#ifndef EMCCONTROLCLASS_H
#define EMCCONTROLCLASS_H

#include "globalDefine.h"
#include "gSoapFoundation/soapStub.h"

enum D_INTERPOLSTION_STATUS
{
    D_INTERPOLSTION_STATUS_NORMAL,
    D_INTERPOLSTION_STATUS_SPEEDLIMIT
};

class WaveMotion;
class CarvingModule;
class MotionMessage;
class EmcControlClass
{
public:
    EmcControlClass(CarvingModule *cavingModuleIn, double arm_warnSlowRatioIn, double cycleIn
                    , MotionMessage *motionMessageIn, double maxLineVelIn, double maxAccIn, int robotIdIn);

    int resetAll();
    int emcmotController();//not used
    int emcmotController_test();
    void set_operating_mode(void);
    void resetJointsCommand(std::vector<double> &jointsIn);
    int getJointCommand(std::vector<double> &jointCommandOut);

//    void printPose(EmcPose poseIn);
    int setNetFeedScale(double scaleIn);
    double getNetFeedScale();

    E_WAVE_STATUS getWaveMotionStatus();
    int setTimeAxisScale(double ratio);
    int startWave();
    int stopWave();
    int ceaseWave();
    int setWaveType(E_WAVE_TYPE typeIn, double waveWidthIn,double wavePeriodIn);
    int setWaveMotionInfo(WeaveMotionCondition infoIn);
    int setWaveAcceleration(double accelerationIn, double maxVelIn, bool isRatio);
private:
    void update_offset_pose(void);
    /* 'update_status()' copies assorted status information to shared
       memory (the emcmotStatus structure) so that it is available to
       higher level code.
    */
    void update_status(void);
    /* 'output_to_hal()' writes the handles the final stages of the
       control function.  It applies screw comp and writes the
       final motor position to the HAL (which routes it to the PID
       loop).  It also drives other HAL outputs, and it writes a
       number of internal variables to HAL parameters so they can
       be observed with halscope and halmeter.
    */
    void output_to_hal(void);
    /* 'compute_screw_comp()' is responsible for calculating backlash and
       lead screw error compensation.  (Leadscrew error compensation is
       a more sophisticated version that includes backlash comp.)  It uses
       the velocity in emcmotStatus->joint_vel_cmd to determine which way
       each joint is moving, and the position in emcmotStatus->joint_pos_cmd
       to determine where the joint is at.  That information is used to
       create the compensation value that is added to the joint_pos_cmd
       to create motor_pos_cmd, and is subtracted from motor_pos_fb to
       get joint_pos_fb.  (This function does not add or subtract the
       compensation value, it only computes it.)  The basic compensation
       value is in backlash_corr, however has makes step changes when
       the direction reverses.  backlash_filt is a ramped version, and
       that is the one that is later added/subtracted from the position.
    */
    void compute_screw_comp(void);
    /* 'handle_jogwheels()' reads jogwheels, decides if they should be
       enabled, and if so, changes the free mode planner's target position
       when the jogwheel(s) turn.
    */

    void handle_jogwheels(void);
    /* 'process_inputs()' is responsible for reading hardware input
       signals (from the HAL) and doing basic processing on them.  In
       the case of position feedback, that means removing backlash or
       screw error comp and calculating the following error.  For
       switches, it means debouncing them and setting flags in the
       emcmotStatus structure.
    */
    void process_inputs(void);
    /* probe inputs need to be handled after forward kins are run, since
       cartesian feedback position is latched when the probe fires, and it
       should be based on the feedback read in on this servo cycle.
    */

    void process_probe_inputs(void);
    int get_pos_cmds_coordinateRescale(long period);//using
    int get_pos_cmds_coordinateSimpleTp(long period);
    int get_pos_cmds_coordinateCubic(long period);
    int get_pos_cmds_coordinate(long period);
    /* 'get_pos_cmds()' generates the position setpoints.  This includes
       calling the trajectory planner and interpolating its outputs.
    */
    void get_pos_cmds(long period);
    int isJointVelPlanOk(int jointIndex);
    int isAllJointReachedTarged();
    double getJointVelOverRatio();

private:
    WaveMotion* waveMotionControl;
    CarvingModule *cavingModule;
    D_INTERPOLSTION_STATUS interpoStatus;
    int innerAxisNum;
    double arm_warnSlowRatio;
    int robotId;
};

#endif // EMCCONTROLCLASS_H
